#include "SDFContact.h"
#include <iostream>
#include <qmath.h>
SDFContact::SDFContact() : osg::Node()
{

}

SDFContact::~SDFContact()
{

}

void SDFContact::addContactPoint(osg::Vec3 &pos, osg::Vec3 &normal)
{
    m_pos.push_back(pos);
    m_normals.push_back(normal);
}

int SDFContact::getContactPointsCount()
{
    return m_pos.size();
}

osg::Vec3 SDFContact::getContactPosByIndex(int index)
{
    osg::Vec3 result;
    if(index >=0 && index < m_pos.size())
    {
        result = m_pos[index];
    }
    return result;
}

osg::Vec3 SDFContact::getContactNormalByIndex(int index)
{
    osg::Vec3 result;
    if(index >=0 && index < m_normals.size())
    {
        result = m_normals[index];
    }
    return result;
}

SDF *SDFContact::getSDF1()
{
    return m_sdf1.get();
}

SDF *SDFContact::getSDF2()
{
    return m_sdf2.get();
}

void SDFContact::setSDF1(osg::ref_ptr<SDF> sdf)
{
    m_sdf1 = sdf;
}

void SDFContact::setSDF2(osg::ref_ptr<SDF> sdf)
{
    m_sdf2 = sdf;
}

bool SDFContact::chargeDirectionIsValid(osg::Vec3 direction)
{
    //std::cout<<"SDFContact::chargeDirectionIsValid direction="<<direction.x()<<","<<direction.y()<<","<<direction.z()<<std::endl;
    bool valid = true;
    for(int i = 0 ; i < m_normals.size(); i++)
    {
        double value = direction*m_normals[i];
        if(qAbs(value - 0) < 0.05)
        {
            valid &= true;
        }
        else if(value > 0)
        {
            valid &= true;
        }
        else if(value < 0)
        {
            if(qAbs(value)<0.34 )//iner 20 degree
                valid &= true;
            else
                valid &= false;
        }

        //std::cout<<"SDFContact::chargeDirectionIsValid normal="<<m_normals[i].x()<<","<<m_normals[i].y()<<","<<m_normals[i].z()<<std::endl;
        //std::cout<<"SDFContact::chargeDirectionIsValid pos="<<m_pos[i].x()<<","<<m_pos[i].y()<<","<<m_pos[i].z()<<std::endl;
        //std::cout<<"SDFContact::chargeDirectionIsValid valid, value="<<valid<<","<<value<<std::endl;
    }
    return valid;
}

bool SDFContact::chargeDirectionIsValid(osg::ref_ptr<SDF> sdfPtr, osg::Vec3 direction)
{
    bool valid = false;
    if(sdfPtr == m_sdf1)
    {
        valid = chargeDirectionIsValid(direction*-1.0);
    }
    else if(sdfPtr == m_sdf2)
    {
        valid = chargeDirectionIsValid(direction);
    }
    else
    {
        valid = false;
    }
    return valid;
}

osg::Vec3 SDFContact::computeResultantForce()
{
    osg::Vec3 resultantForce;
    for(int i = 0; i < m_normals.size(); i++)
    {
        resultantForce += m_normals.at(i);
        //resultantForce.normalize();
    }
    return resultantForce;
}
